//*****************************************************************************
//
// File Name	: ''
// Title		:
// Author		: Andrzej Borowiec
// Created		:
// Revised		:
// Version		:
// Target MCU	:
// Editor Tabs	:
//
// This code is distributed under the GNU Public License
//		which can be found at http://www.gnu.org/licenses/gpl.txt
//
//*****************************************************************************

#ifndef MINIORE32_LINEFOLLOWER01_H
#define MINIORE32_LINEFOLLOWER01_H

//--------------------------------------------------------------------------
// Mini32 Constants - in line follower mode

#define MOTA_DIR2   PD4
#define MOTA_DIR1   PD2
#define MOTA_PWM    PD6

#define MOTB_DIR2   PD7
#define MOTB_DIR1   PB0
#define MOTB_PWM    PD5

#define MOSI        PB3
#define MISO        PB4
#define SCK         PB5

#define BUTTON      PD4
#define BEEP        PD3
#define IR_CTRL     PD2
#define TX          PD1
#define RX          PD0

#define STATUS_LED  PB5

#define L        0
#define R        1

#define P        0
#define I        1
#define D        2

#define LED1     PC7
#define DIR_L    PC2
#define DIR_R    PC3
#define PWM_L    OC1A
#define PWM_R    OC1B
#define MOTOR_L  0
#define MOTOR_R  1
#define FORWARD  1
#define REVERSE  0
#define BACKWARD 0

#define MODE_LINE_FOLLOWER 1
#define MODE_LINE_RACER 0

#define LED_IR PD6
#define IR  J20
#define STAT1 GPIO2
#define STAT2 GPIO3

#define ENC_L PD2
#define ENC_R PD2

#define NUM_OF_ADC_READINGS_TO_AVERAGE 10

#define NUM_OF_SENSORS 6
#define MAX_PWM_COUNT 1000

#define LEDR1 0
#define LEDR2 1
#define LEDR3 2
#define LEDL3 3
#define LEDL2 4
#define LEDL1 5

//--------------------------------------------------------------------------
// State machine definitions
#define ACQUIERING    0
#define TRACKING      1
#define LINE_CROSSING 2
#define LINE_BREAK    3
#define SHARP_TURN_L  4
#define SHARP_TURN_R  5
#define ACUTE_TURN_L  6
#define ACUTE_TURN_R  7
#define EXEC_SHARP_TURN_L  8
#define EXEC_SHARP_TURN_R  9
#define EXEC_ACUTE_TURN_L  10
#define EXEC_ACUTE_TURN_R  11
#define ALL_STOP           12

//--------------------------------------------------------------------------
// Debuging flag
#define DEBUG_MODE    1
#define VERBOSE 0

#define ARROWUP			'A'
#define ARROWDOWN		'B'
#define ARROWRIGHT		'C'
#define ARROWLEFT		'D'
//--------------------------------------------------------------------------
// Debuging flag
#define VT100_RED       91
#define VT100_GREEN     92
#define VT100_YELLOW    93
#define VT100_BLUE      94
#define VT100_MAGENTA   95
#define VT100_CYAN      96

// Truth table for driving TB6552FN/FNG/FL/FLG
// Taken from data sheet page 3
// INPUTS           OUTPUTS MODE
//IN1 IN2 STBY PWM  O1 O2
//H   H   H    H/L  L  L    Short brake
//L   H   H    H    L  H    CW
//H   L   H    H    L  H    CCW
//L   L   H    L    L  H    STOP
//L   H   H    L    L  L    Short brake
//H   L   H    L    L  H    Short brake


// Line follower structures
typedef struct//Hist_t
{
    uint8_t  acquiering,tracking,sharpTurn,lineBreak,lineCrossing;
    uint16_t lineCrossingHistLimit,lineBreakHistLimit;
} Hist_t;

typedef struct//Hist_t EEPRAM
{
    int16_t  max_pwm;
    uint16_t acceleration;
    int16_t  k[3];
    uint8_t  acquiering,tracking,sharp_turn,lineBreak,lineCrossing, acuteTurn;
    uint16_t lineCrossingHistLimit,lineBreakHistLimit;
    uint8_t  num_to_average;
    uint8_t  loop_delay;
    uint16_t acquiering_th, tracking_th, sharpTurn_th,lineBreak_th,lineCrossing_th;
    int16_t  led_coeff[4];
    uint8_t  delta_t;
    uint16_t turn_speed[2];
    uint16_t turn_error_threshod[2];
    uint16_t turn_delay;
    uint16_t trig_reset;
    uint8_t  mode;

    int16_t corr_start_stop1_thresh;
    int16_t corr_start_stop2_thresh;
    int16_t corr_start_stop3_thresh;

} EE_RAM_t;

#define NUM_CORRELATORS 3
typedef struct
{
	int16_t corr_coeff_num[NUM_CORRELATORS][NUM_OF_SENSORS];
	int16_t corr_coeff_denom[NUM_CORRELATORS][NUM_OF_SENSORS];
} EE_RAM_CorrCoeff_t;


typedef struct//PID_t
{
    int16_t  error, prevError, integral, diff;
    int32_t  pwm_inc;
    int16_t  error_back;

    //int16_t  maxPwm;
    //uint16_t acceleration;
    //int16_t  k[3];
} PID_t;

typedef struct//LED_t
{
    uint16_t max[NUM_OF_SENSORS];
    uint16_t val[NUM_OF_SENSORS];
    int16_t signed_val[NUM_OF_SENSORS];
    int16_t approx_sqrt_energy;
    uint16_t min[NUM_OF_SENSORS];
    int16_t norm[NUM_OF_SENSORS];
    uint16_t sum, sumR, sumL, sumB;

} LED_t;


typedef struct//State_t
{
    uint8_t  state;
    uint8_t  previousState;
    uint8_t  currentState;
    uint8_t  trigger;
    uint16_t histIn;
    uint16_t histOut;
    uint16_t histDistance;
    uint16_t pwm[2];
    uint8_t  dir[2];

} State_t;

typedef struct//Encoder_t
{
    uint8_t portd[2];
    int16_t encoder[2];
    int32_t lastEnc[2];
    int16_t deltaEnc[2];
    int16_t totalDistnace;
    int16_t deltaDistnace;
    uint32_t deltaTime[2];
    uint32_t lastTime[2];
} Encoder_t;

typedef struct
{
	int16_t *dataptr;		///< the physical memory address where the buffer is stored
	uint16_t size;			///< the allocated size of the buffer
	uint16_t datalength;
	uint16_t dataindex;		///< the index into the buffer where the data starts
} int16Buffer_t;

void ErrorBufferInit(int16Buffer_t *buffer, int16_t *start, uint16_t size);
unsigned char ErrorBufferAddToEnd(int16Buffer_t* buffer, int16_t data);
void ErrorBufferPrint(int16Buffer_t *buffer);
int16_t ErrorBufferGetValues(int16Buffer_t* buffer, uint16_t index);

//Functions
void RumCmdLine(void);
void SetPwm(uint8_t motor, uint8_t dir, uint16_t pwm);
void ManualPwm(void);
void AllStop(void);
void run(void);
void idn(void);
void InitializeVariables(void);
void ResetVariables(void);
inline void clc(void)
{
    vt100ClearScreen();
    vt100SetCursorPos(1,0);
}
void ReadSensors(void);
int16_t GetPositionError(void);
int16_t GetMeanError(uint8_t window);
void GetPosition(void);
void GetState(void);
void ProcessHysteresis(uint8_t historisitLimit);
void SensorCalibration(void);
void printArray(uint8_t size, uint16_t *array);
void printBin(void);
void printAsci(void);
void SharpTurn(uint8_t dir);
void AcuteTurn(uint8_t dir);
void test(void);
void RunMonitor(void);
void PrintMonitorPage(void);
void GetVar(void);
void SetVar(void);
void manual(void);
void BlinkStatusLed(uint8_t num_of_times, uint16_t delay);
void PrintDebug(uint8_t level, int16_t raw_error);
// functions
//void LEncoderInterrupt(void);
//void REncoderInterrupt(void);

#endif
